// @Author miaoqing
// @Date 2024/11/15 10:31:00
// @Desc
package logic

import (
	"context"
	"core/component/logger"
	"errors"
	"github.com/bluenviron/goroslib/v2"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/actionlib_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
	"strconv"
	"strings"
	"time"
)

type RosClient struct {
	robot        *Robot
	node         *goroslib.Node
	goalPub      *goroslib.Publisher
	statusSub    *goroslib.Subscriber
	reachedGoal  chan bool
	isRunning    bool
	lastCount    int
	currentCount int
	first        bool
	id           int
}

func (rc *RosClient) onGoalResultMessage(statusArray *actionlib_msgs.GoalStatusArray) {
	if rc.first {
		if len(statusArray.StatusList) != 0 {
			id := getMoveBaseGoalIdToInt(statusArray.StatusList[len(statusArray.StatusList)-1].GoalId.Id) + 1
			rc.setMoveBaseInitId(id)
		} else {
			rc.setMoveBaseInitId(1)
		}
		rc.first = false
	}
	if len(statusArray.StatusList) == 0 {
		return
	}
	status := statusArray.StatusList[len(statusArray.StatusList)-1]
	if rc.id != getMoveBaseGoalIdToInt(status.GoalId.Id) {
		return
	}
	if status.Status == actionlib_msgs.GoalStatus_SUCCEEDED {
		rc.reachedGoal <- true
	} else if status.Status == actionlib_msgs.GoalStatus_LOST {
		rc.reachedGoal <- false
	}
}

func newRosClient(robot *Robot) *RosClient {
	rc := &RosClient{
		robot:       robot,
		reachedGoal: make(chan bool),
		first:       true,
	}
	rc.init()
	return rc
}

func (rc *RosClient) init() {
	var err error
	node, err := goroslib.NewNode(goroslib.NodeConf{
		Name:          "golang_ros_node",
		MasterAddress: rc.robot.rosAddress,
	})
	if err != nil {
		logger.Errorf("roscore is not open")
		panic(err)
	}
	goalPub, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Topic: "/move_base_simple/goal",
		Msg:   &geometry_msgs.PoseStamped{},
		Node:  node,
	})
	if err != nil {
		return
	}
	statusSub, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:     node,
		Topic:    "/move_base/status",
		Callback: rc.onGoalResultMessage,
	})
	if err != nil {
		return
	}
	rc.statusSub = statusSub
	rc.goalPub = goalPub
	rc.node = node
	logger.Info("rosClient Init Success")
}

func (rc *RosClient) waitForGoalReached() {
	logger.Info("rosClient Wait for Goal Reached")
	ctx, cancel := context.WithTimeout(context.Background(), 600*time.Second)
	defer cancel()
	select {
	case ret := <-rc.reachedGoal:
		rc.robot.reachedGoal <- ret
		logger.Info("Goal reached successfully!")
	case <-ctx.Done():
		rc.robot.reachedGoal <- false
		logger.Error("Failed to reach goal:", ctx.Err())
	}
	rc.isRunning = false
	rc.id++
	logger.Info("rosClient wait goal end")
}

func (rc *RosClient) sendGoal(x, y, w float64) error {
	if rc.goalPub == nil || rc.statusSub == nil || rc.isRunning {
		return errors.New("rosClient is running")
	}
	goal := &geometry_msgs.PoseStamped{
		Header: std_msgs.Header{
			FrameId: "map",
			Stamp:   time.Now(),
		},
		Pose: geometry_msgs.Pose{
			Position: geometry_msgs.Point{
				X: x,
				Y: y,
			},
			Orientation: geometry_msgs.Quaternion{
				W: w,
			},
		},
	}
	rc.currentCount++
	logger.Infof("Sent goal: (%f, %f, %f)\n", x, y, w)
	rc.goalPub.Write(goal)
	rc.isRunning = true
	rc.waitForGoalReached()
	return nil
}

func (rc *RosClient) Close() {
	if rc.node == nil {
		return
	}
	logger.Info("rosClient Close")
	rc.goalPub.Close()
	rc.statusSub.Close()
	rc.node.Close()
	rc.statusSub = nil
	rc.goalPub = nil
	rc.node = nil
}

func (rc *RosClient) setMoveBaseInitId(id int) {
	rc.id = id
	logger.Info("rosClient set move base init id:", id)
}

func getMoveBaseGoalIdToInt(idStr string) int {
	ids := strings.Split(idStr, "-")
	if len(ids) == 0 {
		return 0
	}
	n, _ := strconv.Atoi(ids[1])
	return n
}
